#include "robot_manager.h"
#include "utilities/file.hpp"
void RobotManager::commandJointScanCallback(const std_msgs::Float64MultiArray::ConstPtr &msg)
{
    if (msg->data.size() == 4)
    {
        ROS_INFO_STREAM("[Joint Scan] Received command: "
                        << "Joint ID: " << msg->data[0] << ", "
                        << "Joint freq: " << msg->data[1] << ", "
                        << "Joint Amplitude: " << msg->data[2] << ", "
                        << "Scan cycle: " << msg->data[3]);
    }
    else
    {
        ROS_WARN("[Joint Scan] Received command does not contain 4 values.");
    }

    std::vector<double> mit_kp, mit_kd;
    nh_.getParam("mit_kp", mit_kp);
    nh_.getParam("mit_kd", mit_kd);
    std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};

    pthread_mutex_lock(&zoros_mutex_);
    std::vector<double> init_position = joint_state_msg_.position;
    pthread_mutex_unlock(&zoros_mutex_);

    ros::Rate rate(CONTROL_FREQ);
    double t = 0.0;
    std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;

    std::vector<std::vector<double>> actual_position_sets;
    std::vector<std::vector<double>> target_position_sets;
    while (1)
    {
        pthread_mutex_lock(&zoros_mutex_);
        q = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);
        if(t > msg->data[3] / msg->data[1])
        {
            break;
        }

        pthread_mutex_lock(&zoros_mutex_);
        robot_model_->inverseDynamics(q, qd, qdd, torque);
        double q_target = msg->data[2] * sin(2 * M_PI * msg->data[1] * t);
        double qd_target = 2 * M_PI * msg->data[1] * msg->data[2] * cos(2 * M_PI * msg->data[1] * t);

        for (size_t i = 0; i < 7; i++)
        {
            if (!is_simulation_)
            {
                if(i == (int)msg->data[0])
                {
                    dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], q_target, qd_target, torque[i] * D[i]);
                    cmd_msg_.position[i] = q_target;
                }
                else
                {
                    dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], init_position[i], 0.0, torque[i] * D[i]);
                    cmd_msg_.position[i] = init_position[i];
                }
            }
            else
            {
                if(i == (int)msg->data[0])
                {
                    joint_state_msg_.position[i] = q_target;
                }
                
            }
        }
        if(!is_simulation_)
        {
            cmd_msg_.header.stamp = ros::Time::now();
            cmd_pub_.publish(cmd_msg_); // 发布初始的 joint_states 消息
        }
        else
        {
            //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
            joint_state_msg_.header.stamp = ros::Time::now();
            joint_pub_.publish(joint_state_msg_); // 发布初始的 joint_states 消息
        }
        target_position_sets.push_back(cmd_msg_.position);
        actual_position_sets.push_back(joint_state_msg_.position);
        pthread_mutex_unlock(&zoros_mutex_);

        t += 1.0 / CONTROL_FREQ;
        rate.sleep();
    }
    WriteData2D("./target.txt", target_position_sets, ',');
    WriteData2D("./actual.txt", actual_position_sets, ',');
    ROS_INFO("scan over.");
}